#include "Package.h"

#define NR_RETRANSMISSION 5

typedef struct FifoData {
	int16_t id;
	int count;
} FifoData;

module PackageIDFifoC {
	provides interface PackageIDFifo as Fifo;
	uses interface Queueing<FifoData> as Q;
	uses interface LEDControl;
	uses interface Data;
}
implementation {

	FifoData element;
	int counterForRemoved = 0;
	bool idRemovedLed = TRUE;

	command int16_t Fifo.dequeue() {
		element = call Q.dequeue();
		return element.id;
	}

	command void Fifo.enqueue(int16_t x) {
		int i, size;
		bool elementEnqueued = FALSE;
		size = call Q.size();
		for(i = 0; i < size; i++) {
			element = call Q.dequeue();
			if(element.id == x) {
				element.count++;
				if(element.count <= NR_RETRANSMISSION) 
					call Q.enqueue(element);
				else {
					counterForRemoved++;
					if(idRemovedLed == TRUE) 
						call LEDControl.On(LEDState_ModePassive);
					else 
						call LEDControl.Off(LEDState_ModePassive);
					idRemovedLed = ! idRemovedLed;
				}
				elementEnqueued = TRUE;
			}
			else 
				call Q.enqueue(element);
		}
		if(elementEnqueued == FALSE) {
			element.id = x;
			element.count = 0;
			call Q.enqueue(element);
		}
	}

	command int Fifo.remove(int16_t idx) {
		int i, size;
		int removed = 0;
		size = call Q.size();
		for(i = 0; i < size; i++) {
			element = call Q.dequeue();
			if(element.id == idx) 
				removed = element.count;
			else 
				call Q.enqueue(element);
		}
		return removed;
	}

	command void Fifo.peek(int16_t idArray[], int size) {
		int i;
		for(i = 0; i < size; i++) {
			element = call Q.dequeue();
			idArray[i] = element.id;
			call Q.enqueue(element);
		}
	}

	command void Fifo.trashAll() {
		int i;
		int size = call Q.size();

		for(i = 0; i < size; i++) {
			call Q.dequeue();
		}
		counterForRemoved = 0;
	}

	command bool Fifo.empty() {
		return call Q.empty();
	}

	command int Fifo.size() {
		return call Q.size();
	}

	command int Fifo.status() {
		return counterForRemoved;
	}
}